// SPDX-License-Identifier: BSD-2-Clause

#include <laser_scan_mapping/map_cloud_generator.h>

#include <pcl/octree/octree_search.h>

namespace laser_scan_graph_slam {

MapCloudGenerator::MapCloudGenerator() {}

MapCloudGenerator::~MapCloudGenerator() {}

pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(const std::vector<KeyFrameSnapshot::Ptr>& keyframes, double resolution) const {
  if(keyframes.empty()) {
    std::cerr << "warning: keyframes empty!!" << std::endl;
    return nullptr;
  }

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
  cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());

  for(const auto& keyframe : keyframes) {
    Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
    for(const auto& src_pt : keyframe->cloud->points) {
      PointT dst_pt;
      dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
      dst_pt.intensity = src_pt.intensity;
      cloud->push_back(dst_pt);
    }
  }

  cloud->width = cloud->size();
  cloud->height = 1;
  cloud->is_dense = false;

  if (resolution <=0.0)
    return cloud; // To get unfiltered point cloud with intensity

  pcl::octree::OctreePointCloud<PointT> octree(resolution);
  octree.setInputCloud(cloud);
  octree.addPointsFromInputCloud();
  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
  octree.getOccupiedVoxelCenters(filtered->points);

  // pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);//输出点云
	// pcl::VoxelGrid<PointT> sor; //创建滤波器
	// sor.setInputCloud(cloud);//待处理点云cloud放进去
	// sor.setLeafSize(resolution, resolution, resolution);//三维格网或者叫体素格网的长宽高
	// sor.setMinimumPointsNumberPerVoxel(int(resolution*100));//设置要使用的体素所需的最小点数  设置过大程序崩溃，因为都不满足就没点输出，保存会错误
	// sor.filter(*filtered);//接受滤波处理后的点云容器放进去

  filtered->width = filtered->size();
  filtered->height = 1;
  filtered->is_dense = false;

  return filtered;
}

}  // namespace laser_scan_graph_slam


